
#include "MY_INC/PHY/PHY_Integrator.h"

/*==================================积分算法===================================*/
/**
 * @brief Tustin积分器初始化函数
 * @param intg 积分器结构体指针
 * @param ts 采样周期
 */
static inline void Tustin_Integrator_Reset_Inline(TustinIntegral_Typedef *intg);
void               Tustin_Integrator_Init(TustinIntegral_Typedef *intg, float ts)
{
    if (intg == NULL)
        return;                           // 检查指针是否为 NULL
    intg->gain = ts * 0.5f;               // 确保增益为 T/2
    Tustin_Integrator_Reset_Inline(intg); // 重置积分器状态
}
void Tustin_Integrator_Reset(TustinIntegral_Typedef *intg)
{
    if (intg == NULL)
        return;                           // 检查指针是否为 NULL
    Tustin_Integrator_Reset_Inline(intg); // 重置积分器状态
}
static inline void Tustin_Integrator_Reset_Inline(TustinIntegral_Typedef *intg)
{
    intg->input_prev = 0.0f;
    intg->output_prev = 0.0f;
}
/**
 * @brief Tustin积分器计算函数
 * @param intg 积分器结构体指针
 * @param input 输入值
 * @return 积分器输出值
 */
float Tustin_Integrator(TustinIntegral_Typedef *intg, float input)
{
    // 计算输出
    float output = intg->output_prev + intg->gain * (input + intg->input_prev);
    // 更新状态
    intg->input_prev = input;
    intg->output_prev = output;
    return output;
}

/**
 * @brief 后向积分器初始化函数
 * @param Integral 积分器结构体指针
 * @param ts 采样周期
 */
void Backward_Integrator_Init(BackwardIntegral_Typedef *Integral, float ts)
{
    Integral->accumulated = 0.0f;
    Integral->T = ts;
}

/**
 * @brief 后向积分器计算函数
 * @param Integral 积分器结构体指针
 * @param x_n 输入值
 * @return 积分器输出值
 */
float Backward_Integrate(BackwardIntegral_Typedef *Integral, float x_n)
{
    Integral->accumulated += Integral->T * x_n; // 累加 y[n] = y[n-1] + T*x[n]
    return Integral->accumulated;
}

/*==================================PLL算法===================================*/

/**
 * @brief PLL初始化函数
 * @param PLL PLL结构体指针
 * @param Kp 比例增益
 * @param Ki 积分增益
 * @param Feedforward 前馈值
 * @param ts 采样周期
 */